
#ifndef MY_INC_PHY_PHY_IIRFILTER_H_
#define MY_INC_PHY_PHY_IIRFILTER_H_

#include <stdint.h>
#include <stdio.h>
#include <string.h>
// 2P2Z滤波器系数结构体
typedef struct
{
    float Coeff_B0;
    float Coeff_B1;
    float Coeff_B2;
    float Coeff_A1;
    float Coeff_A2;

    float Xn_1; // 输入信号Xn-1
    float Xn_2; // 输入信号Xn-2
    float Yn;   // 输出信号Yn-1
    float Yn_1; // 输出信号Yn-1
    float Yn_2; // 输出信号Yn-2
} Coeff_2P2Z;

void Coeff2P2Z_Init(Coeff_2P2Z *_2P2Z, float Coeff_B0, float Coeff_B1, float Coeff_B2, float Coeff_A1, float Coeff_A2);
void Coeff2P2Z_Reset(Coeff_2P2Z *_2P2Z);

static inline float Diff2P2Z_Filter_Inline(Coeff_2P2Z *_2P2Z, float Xn)
{
    _2P2Z->Yn = _2P2Z->Coeff_B0 * Xn + _2P2Z->Coeff_B1 * _2P2Z->Xn_1 + _2P2Z->Coeff_B2 * _2P2Z->Xn_2 -
                _2P2Z->Coeff_A1 * _2P2Z->Yn_1 - _2P2Z->Coeff_A2 * _2P2Z->Yn_2;
    // 更新历史缓冲区
    _2P2Z->Xn_2 = _2P2Z->Xn_1; // Xn-2 = Xn-1
    _2P2Z->Xn_1 = Xn;          // Xn-1 = Xn
    _2P2Z->Yn_2 = _2P2Z->Yn_1; // Yn-2 = Yn-1
    _2P2Z->Yn_1 = _2P2Z->Yn;   // Yn-1 = Yn

    return _2P2Z->Yn; // 返回当前输出信号
}

// 2P2Z滤波器系数结构体
typedef struct
{
    float Coeff_B0;
    float Coeff_B1;
    float Coeff_A1;

    float Xn_1; // 输入信号Xn-1
    float Yn_1; // 输出信号Yn-1
} Coeff_1P0Z;

void Coeff1P0Z_Init(Coeff_1P0Z *_1P0Z, float Coeff_B0, float Coeff_B1, float Coeff_A1);
void Coeff1P0Z_Reset(Coeff_1P0Z *_1P0Z);

static inline float Diff1P0Z_Filter_Inline(Coeff_1P0Z *_1P0Z, float Xn)
{
    float Yn = (_1P0Z->Coeff_B0 * Xn) + (_1P0Z->Coeff_B1 * _1P0Z->Xn_1) - (_1P0Z->Coeff_A1 * _1P0Z->Yn_1);
    _1P0Z->Xn_1 = Xn; // Xn-1 = Xn
    _1P0Z->Yn_1 = Yn; // Yn-1 = Yn
    return Yn;        // 返回当前输出信号
}

#endif